#include "CoordSys.h"
#include <cmath>

CoordSys::CoordSys() = default;

void CoordSys::XYZ2BLH(const XYZ& xyz, BLH& blh) {
    double p = std::sqrt(xyz.x * xyz.x + xyz.y * xyz.y);
    double b = a * (1 - f);
    double e2 = 2 * f - f * f;
    double theta = std::atan2(xyz.z * a, p * b);

    blh.L = std::atan2(xyz.y, xyz.x);
    blh.B = std::atan2(xyz.z + (e2 * b / std::sqrt(1 - e2)) * std::pow(std::sin(theta), 3),
        p - a * e2 * std::pow(std::cos(theta), 3));

    double N = a / std::sqrt(1 - e2 * std::sin(blh.B) * std::sin(blh.B));
    blh.H = p / std::cos(blh.B) - N;
}

void CoordSys::BLH2XYZ(const BLH& blh, XYZ& xyz) {
    double e2 = 2 * f - f * f;
    double N = a / std::sqrt(1 - e2 * std::sin(blh.B) * std::sin(blh.B));

    xyz.x = (N + blh.H) * std::cos(blh.B) * std::cos(blh.L);
    xyz.y = (N + blh.H) * std::cos(blh.B) * std::sin(blh.L);
    xyz.z = (N * (1 - e2) + blh.H) * std::sin(blh.B);
}

void CoordSys::XYZ2ENU(const XYZ& xyz, const XYZ& ref, ENU& enu) {
    BLH refBLH;
    XYZ2BLH(ref, refBLH);

    double dx = xyz.x - ref.x;
    double dy = xyz.y - ref.y;
    double dz = xyz.z - ref.z;

    double sinB = std::sin(refBLH.B);
    double cosB = std::cos(refBLH.B);
    double sinL = std::sin(refBLH.L);
    double cosL = std::cos(refBLH.L);

    enu.e = -sinL * dx + cosL * dy;
    enu.n = -sinB * cosL * dx - sinB * sinL * dy + cosB * dz;
    enu.u = cosB * cosL * dx + cosB * sinL * dy + sinB * dz;
}

XYZ CoordSys::XYZ_subtract(const XYZ& a, const XYZ& b) {
    return { a.x - b.x, a.y - b.y, a.z - b.z };
}

double CoordSys::XYZ_norm(const XYZ& xyz) {
    return std::sqrt(xyz.x * xyz.x + xyz.y * xyz.y + xyz.z * xyz.z);
}